//
// Created by juangeng on 21/08/19.
//

#ifndef RL_C1_DRIVER_IP_TCP_CONNECT_H
#define RL_C1_DRIVER_IP_TCP_CONNECT_H

#include <boost/thread.hpp>
#include <boost/asio.hpp>
#include <mutex>
#include <sys/msg.h>
#include <fstream>

#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose2D.h>
#include <std_msgs/Float32.h>
#include "nav_msg.h"
#include "insprobe_msgs/Navigation.h"

using namespace boost::asio;
typedef boost::shared_ptr<ip::tcp::socket> socket_ptr;

#define MAX_TCP_SIZE 64
#define TCPPORT      9999

class Server {
public:
    Server(int _tcpport, ros::NodeHandle nodeHandle)
            : tcpport_(_tcpport), tcprxcnt_(0), node_(nodeHandle),
              voltage_(0), battery_(0), target_lat_(0), target_lon_(0){

        boost::thread serv(&Server::serverThread, this);
        serv.detach();
    }

    ~Server() {
        msg_thread_.join();

        acceptor_->close();
        delete acceptor_;

        ioservice_.stop();
        ioservice_.reset();

        socket_.reset();
    }
    void run();
protected:
    void getLocationMsg(const insprobe_msgs::Navigation::ConstPtr &msg);
    void pathPointCallback(const nav_msgs::Path::ConstPtr &msg);
    void getPathMsg(std::vector<double>& _lat, std::vector<double>& _lon, int _mode);
    void batteryVCallback(const std_msgs::Float32::ConstPtr &msg);
    void batteryPCallback(const std_msgs::Float32::ConstPtr &msg);
private:
    void serverThread();
    void startAccept();
    void startRead();
    void acceptHandler(socket_ptr sock, const boost::system::error_code &err);
    void readHandler(const boost::system::error_code &err, std::size_t bytes);
    void sendBattery(const ros::TimerEvent &event);
private:
    ros::NodeHandle node_;
    boost::thread   msg_thread_;

    io_service ioservice_;
    ip::tcp::acceptor *acceptor_;
    socket_ptr socket_;

    int tcpport_;
    float voltage_;
    float battery_;

    std::mutex sockmutex_;
    std::mutex msgmutex_;

    char tcptxbuff_[256];
    char tcprxbuff_[256];
    char tcprxtemp_[256];
    int tcprxcnt_;

    ros::Publisher cmd_pub_, Pose2D_pub_;
    geometry_msgs::Twist current_twist_;
    geometry_msgs::Pose2D target_pose2d_;

    double target_lat_, target_lon_;

    InsprobePack insprobe_pack_;
};
#endif //RL_C1_DRIVER_IP_TCP_CONNECT_H
